﻿using System;
using Microsoft.SPOT;
using Mountaineer.Netmf.Hardware;
using System.Threading;
using Microsoft.SPOT.Hardware;
using TI_BLE_HCI_ClientLib.Controllers;
using TI_BLE_HCI_ClientLib.DataEntities.Advertising;
using System.Collections;
using TI_BLE_HCI_ClientLib.DataEntities.Advertising.ManufacturerSpecificDataEntities;
using uPLibrary.Networking.M2Mqtt;
using uPLibrary.Networking.M2Mqtt.Messages;
using System.Text;

namespace iBeaconScanner_NETMF
{
    public static class Program
    {
        const string BROKER_ADDR = "192.168.0.201";
        const string CLIENT_IDENTIFIER = "N_DEVICE_1";
        const string PUB_TOPIC = "/ibeacon/scanner/";

        const int SCAN_INTERVAL_MSECS = 2000;
        const int SCAN_INTERVAL_TIMEOUT_MSECS = 4000;
        private const int MAX_DETECTED_BLE_DEVICES = 20;

        //const int SUPERVISORY_TIMEOUT_MSECS = 1000 * 60 * 60;

        private const double RANGE_THRESHOLD_OUT_TO_IN = 2.0;
        private const double RANGE_THRESHOLD_IN_TO_OUT = 10.0;

        static readonly AutoResetEvent _are = new AutoResetEvent(false);
        static readonly Hashtable _iBeacons = new Hashtable();
        static readonly Hashtable _iBeaconsPreviousState = new Hashtable();
        static HCISimpleSerialCentralProfileController _controller;
        static MqttClient _mqttClient;
        static Thread _connectionThread;

        static OutputPort _ledB = new OutputPort(Mountaineer.Netmf.Hardware.OnboardIO.LedBlue, false);
        static OutputPort _ledR = new OutputPort(Mountaineer.Netmf.Hardware.OnboardIO.LedRed, false);
        static OutputPort _ledG = new OutputPort(Mountaineer.Netmf.Hardware.OnboardIO.LedGreen, false);

        //static Thread _supervisoryThread;

        //static AutoResetEvent _areSupervisory = new AutoResetEvent(false);

        public static void Main()
        {
            if (!initController()) return;

            _controller.OnDeviceInformation += _controller_OnDeviceInformation;
            _controller.OnDeviceDiscoveryDone += _controller_OnDeviceDiscoveryDone;

            try
            {
                initMqtt();
            }
            catch (Exception ex)
            {
                Debug.Print("Inizializzazione MQTT fallita: " + ex.ToString());

                return;
            }

            while (true)
            {
                if (_mqttClient != null && _mqttClient.IsConnected) break;

                Thread.Sleep(1000);
            }

            //initSupervisoryThread();

            while (true)
            {
                lock (_iBeacons.SyncRoot)
                {
                    _iBeacons.Clear();
                }

                _controller.BeginDeviceDiscovery();

                bool ok = _are.WaitOne(SCAN_INTERVAL_TIMEOUT_MSECS, false);

                if (!ok)
                {
                    if (!_controller.EndDeviceDiscovery())
                    {
                        Debug.Print("EndDeviceDiscovery FAILED !");
                    }
                }
            }
        }

        static void initMqtt()
        {
            _connectionThread = new Thread(connectionThreadProc);

            _connectionThread.Priority = ThreadPriority.Lowest;

            _connectionThread.Start();
        }

        /*static void initSupervisoryThread()
        {
            _supervisoryThread = new Thread(supervisoryThreadThreadProc) { Priority = ThreadPriority.Lowest };

            _supervisoryThread.Start();
        }

        static void supervisoryThreadThreadProc()
        {
            while (true)
            {
                if (!_areSupervisory.WaitOne(SUPERVISORY_TIMEOUT_MSECS, false))
                {
                    _ledR.Write(true);
                    Thread.Sleep(1000);

                    PowerState.RebootDevice(false);

                    return;
                }
            }
        }*/

        static void connectionThreadProc()
        {
            while (true)
            {
                if (_mqttClient != null && _mqttClient.IsConnected)
                {
                    _ledB.Write(true);
                    Thread.Sleep(100);
                    _ledB.Write(false);
                    Thread.Sleep(10000);

                    continue;
                }

                try
                {
                    _ledB.Write(true);
                    Debug.Print("Connecting to MQTT Broker...");

                    if (connect())
                    {
                        Debug.Print("...connection to MQTT Broker OK");
                        _ledB.Write(false);
                    }
                    else
                    {
                        Debug.Print("...connection to MQTT Broker FAILED");
                        _ledR.Write(true);
                        Thread.Sleep(100);
                        _ledR.Write(false);
                    }
                }
                catch { }

                Thread.Sleep(5000);
            }
        }
        static bool connect()
        {
            _mqttClient = new MqttClient(BROKER_ADDR);

            bool ok = _mqttClient.Connect(CLIENT_IDENTIFIER) == MqttMsgConnack.CONN_ACCEPTED;

            return ok;
        }

        static void _controller_OnDeviceDiscoveryDone(TI_BLE_HCI_ClientLib.HCIEvents.HCIEvent_GAP_Device_Discovery_Done deviceDiscoveryDone)
        {
            try
            {
                publishDetectedDevices();
            }
            finally
            {
                _are.Set();
            }
        }

        private static void publishDetectedDevices()
        {
            Debug.Print("--- NEW PUBLISHING ROUND: ---");

            var addrsToRemove = new ArrayList();

            lock (_iBeacons.SyncRoot)
            {
                Debug.Print("Current iBeacons Keys:");

                foreach (var addr in _iBeacons.Keys)
                {
                    Debug.Print("\t" + addr);
                }

                Debug.Print("\nPrevious iBeacons Keys:");

                foreach (var addr in _iBeaconsPreviousState.Keys)
                {
                    Debug.Print("\t" + addr);
                }

                foreach (var addr in _iBeacons.Keys)
                {
                    var iBeacon = (_iBeacons[addr] as iBeaconData);

                    if (iBeacon == null) continue;

                    if (iBeacon.Addr == null) continue;

                    // Presence
                    publishiBeaconDetected(iBeacon);

                    Debug.Print("/////////////////////////////////////////");

                    Debug.Print(addr + " range: " + iBeacon.Range + " ...");

                    // Manage delta state
                    if (_iBeaconsPreviousState.Contains(addr))
                    {
                        Debug.Print("...già presente...");

                        var previousBeaconData = (_iBeaconsPreviousState[addr] as iBeaconData);

                        if (previousBeaconData == null) continue;

                        if (iBeacon.Range < RANGE_THRESHOLD_IN_TO_OUT)
                        {
                            Debug.Print("... entro il range IN TO OUT...");
                            previousBeaconData.DeltaScanningState = iBeaconData.DeltaScanningStateEnum.AlreadyDetected;
                        }
                        else
                        {
                            Debug.Print("... fuori range IN TO OUT...");

                            addrsToRemove.Add(addr);
                        }
                    }
                    else
                    {
                        Debug.Print("...non presente...");

                        if (iBeacon.Range < RANGE_THRESHOLD_OUT_TO_IN)
                        {
                            Debug.Print("... entro il range OUT TO IN...");

                            _iBeaconsPreviousState[addr] = iBeacon;
                            iBeacon.DeltaScanningState = iBeaconData.DeltaScanningStateEnum.In;
                        }
                        else
                        {
                            Debug.Print("... fuori range OUT TO IN...");
                        }
                    }
                }
            }

            foreach (var addr in _iBeaconsPreviousState.Keys)
            {
                var iBeacon = (_iBeaconsPreviousState[addr] as iBeaconData);

                if (iBeacon == null) continue;

                switch (iBeacon.DeltaScanningState)
                {
                    case iBeaconData.DeltaScanningStateEnum.In:
                        publishBeaconIn(iBeacon);
                        break;

                    case iBeaconData.DeltaScanningStateEnum.AlreadyDetected:
                        publishBeaconAlreadyDetected(iBeacon);
                        break;

                }
            }

            foreach (var addr in addrsToRemove)
            {
                Debug.Print("Rimuovo " + addr);

                _iBeaconsPreviousState.Remove(addr);
            }

            Debug.Print("^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^");

        }

        private static void publishBeaconIn(iBeaconData iBeacon)
        {
            if (_mqttClient == null || !_mqttClient.IsConnected) return;

            _mqttClient.Publish(PUB_TOPIC + CLIENT_IDENTIFIER + "/in", Encoding.UTF8.GetBytes(buildiBeaconMessage(iBeacon)), MqttMsgBase.QOS_LEVEL_AT_LEAST_ONCE, false);
        }


        private static void publishBeaconAlreadyDetected(iBeaconData iBeacon)
        {
            if (_mqttClient == null || !_mqttClient.IsConnected) return;

            _mqttClient.Publish(PUB_TOPIC + CLIENT_IDENTIFIER + "/already-detected", Encoding.UTF8.GetBytes(buildiBeaconMessage(iBeacon)), MqttMsgBase.QOS_LEVEL_AT_LEAST_ONCE, false);
        }

        static void publishiBeaconDetected(iBeaconData iBeacon)
        {
            if (_mqttClient == null || !_mqttClient.IsConnected) return;

            _mqttClient.Publish(PUB_TOPIC + CLIENT_IDENTIFIER + "/detected", Encoding.UTF8.GetBytes(buildiBeaconMessage(iBeacon)), MqttMsgBase.QOS_LEVEL_AT_LEAST_ONCE, false);
        }

        static string buildiBeaconMessage(iBeaconData iBeacon)
        {
            Hashtable ht = new Hashtable();

            ht["Addr"] = iBeacon.Addr.ToHexStringReversed();
            ht["DeviceName"] = iBeacon.DeviceName;
            ht["Major"] = iBeacon.Major;
            ht["Minor"] = iBeacon.Minor;
            ht["Rssi"] = iBeacon.Rssi;
            ht["TxCalibratedPower"] = iBeacon.TxCalibratedPower;
            ht["IsAppleiBeacon"] = iBeacon.IsAppleiBeacon;
            ht["Range"] = System.Math.Round(iBeacon.Range * 10) / 10;

            return Json.NETMF.JsonSerializer.SerializeObject(ht);
        }

        static void _controller_OnDeviceInformation(TI_BLE_HCI_ClientLib.HCIEvents.HCIEvent_GAP_Device_Information deviceInfo)
        {


            //Debug.Print("DEVICE INFO (ADDR=" + deviceInfo.Addr.ToHexStringReversed() + "):");

            //_areSupervisory.Set();

            try
            {
                if (parseAndStoreBeaconData(deviceInfo))
                {
                    _ledG.Write(true);
                    Thread.Sleep(50);
                    _ledG.Write(false);
                }
                else
                {
                    _ledR.Write(true);
                    Thread.Sleep(50);
                    _ledR.Write(false);
                }
            }
            catch { }
        }

        static bool parseAndStoreBeaconData(TI_BLE_HCI_ClientLib.HCIEvents.HCIEvent_GAP_Device_Information deviceInfo)
        {
            string key = deviceInfo.Addr.ToHexStringReversed();

            lock (_iBeacons.SyncRoot)
            {
                iBeaconData iBeacon = new iBeaconData();

                foreach (var ad in deviceInfo.AdvertisingAndScanResponseData.AdvertisingDataStructures)
                {
                    if (ad is ManufacturerSpecificDataAdvertisingDataStructure)
                    {
                        var mfad = ad as ManufacturerSpecificDataAdvertisingDataStructure;

                        if (ManufacturerSpecificData_iBeacon.CanBeCreatedFromManufacturerSpecificData(mfad))
                        {
                            var ibmfad = ManufacturerSpecificData_iBeacon.CreateFromManufacturerSpecificData(mfad);

                            if (ibmfad == null) continue;

                            iBeacon.Addr = deviceInfo.Addr;
                            iBeacon.Major = ibmfad.Major;
                            iBeacon.Minor = ibmfad.Minor;
                            iBeacon.Rssi = deviceInfo.Rssi;
                            iBeacon.TxCalibratedPower = ibmfad.TxCalibratedPower;
                            iBeacon.IsAppleiBeacon = ibmfad.IsAppleiBeacon;
                        }
                    }
                    else if (ad is LocalNameAdvertisingDataStructure)
                    {
                        var localnamead = ad as LocalNameAdvertisingDataStructure;

                        iBeacon.DeviceName = localnamead.DeviceName;
                    }
                }

                if (iBeacon.IsAppleiBeacon)
                {
                    _iBeacons[key] = iBeacon;
                }
            }

            return false;
        }

        static bool initController()
        {
            _controller = new HCISimpleSerialCentralProfileController(Socket3.SerialPortName, 115200);

            _controller.Start();

            if (!_controller.Reset(HCIController.ResetTypeEnum.HardReset))
            {
                Debug.Print("Controller Reset FAILED!");
                return false;
            }

            if (!_controller.Init(MAX_DETECTED_BLE_DEVICES))
            {
                Debug.Print("Controller Init FAILED!");
                return false;
            }

            if (!_controller.SetGAPParameter(HCIController.ParamIDEnum.TGAP_GEN_DISC_SCAN, SCAN_INTERVAL_MSECS))
            {
                Debug.Print("Controller Set GAP Parameter FAILED!");
                return false;
            }

            return true;
        }
    }
}
